// 无人机状态机
/******************************* 状态转移图 **********************************
 * 
 * 
 * 
INITILIZED SYSTEM ---> AUTO.TAKEOFF----> HOLD(POSITION)------> OFFBOARD
        ^                                 |   | ^    ^             |
        |                                 |   | |    |-------------|
        |                                 |   | |
        |                                 |   V |
        |<------------ AUTO.LAND <--------| MISSION
        |                                 |
        |                                 |
        |<------------ AUTO.RETURN <------|
 *
 *
 * 
***************************************************************************/

// 状态机类
class StateMachine {
private:
    State* current_state;

public:
    StateMachine(State* state) : current_state(state) {}

    ~StateMachine() {
        delete current_state;
    }

    void Run() {
        while (true) {
            current_state->Handle();
            State* next_state = current_state->Next();
            delete current_state; // 删除当前状态
            current_state = next_state; // 切换到下一个状态
        }
    }
};

		public override void _Ready()
		{
			m_currentState = "Idle";
			m_lastState = "Idle";
			m_newState = m_currentState;
		}

		public override void _Process(double delta)
		{
			TransferState();
			UnitStateDictionary.Instance.m_stateDictionary[m_currentState].UpdataState(m_currentUnit);
		}

		private void TransferState()
		{
			if (m_newState != m_currentState)
			{
				UnitStateDictionary.Instance.m_stateDictionary[m_currentState].LeaveState(m_currentUnit);
				m_currentState = m_newState;
				UnitStateDictionary.Instance.m_stateDictionary[m_currentState].EnterState(m_currentUnit);
			}
		}

		public void SetUnitState(string newState)
		{
			if (newState != m_currentState)
			{
				m_newState = newState;
				m_lastState = m_currentState;
			}
		}

		public string GetLastState()
		{
			return m_lastState;
		}


int main() {
    StateMachine machine(new State1()); // 创建状态机，初始状态为State1
    machine.Run(); // 运行状态机

    return 0;
}